$TITLE ('MEMORY-POOL FUNCTION CALLS')
$SYMBOLS
$NOXREF
$NOCOND
$NOMOD51
$NORB
$PAGELENGTH(80) PAGEWIDTH(110)
;************************************************************************
;*                                                                      *
;*    R T X - 5 1  :   MEMORY-POOL FUNCTION CALLS                       *
;*                                                                      *
;*----------------------------------------------------------------------*
;*                                                                      *
;*    Filename     :   RTXPOOL.A51                                      *
;*    Language     :   Keil A-51                                        *
;*    Dev. system  :   Keil uVision2                                    *
;*    Targetsystem :   Any system based upon 8051 up                    *
;*                                                                      *
;*    Purpose      :   Implements the os_create_pool, the os_get_block  *
;*                     and the os_free_block function calls.            *
;*                                                                      *
;*----------------------------------------------------------------------*
;* Rev. |  Released   | Programmer    | Comments                        *
;*----------------------------------------------------------------------*
;* 0.1  |  20.9.1991  | Th. Fischler  | Module creation for RTX-51 V4.0 *
;* 0.2  |   9.4.1992  |               | Error with blocks >255 Bytes    *
;*      |             |               | corrected                       *
;*      |  24.3.1993  | Th. Fischler  | Function return for get_block   *
;*      |             |               | corrected (NFL_EVENT problem)   *
;* 5.00 | 20.10.1994  | E. Glatz      | Release 5.00                    *
;* 5.10 | 19.04.1996  |               | Release ctr struct if create err*
;* 7.00 | 13.11.2001  | T. Mueller    | Release 7.00                    *
;************************************************************************
;*    Copyright 1991 .. 2001 METTLER  &  FUCHS  AG,  CH-8953 Dietikon   *
;************************************************************************


$INCLUDE (RTXDEBUG.INC)
$INCLUDE (RTXIMASK.INC)


;*----------------------------------------------------------------------*
;*      I M P O R T S
;*----------------------------------------------------------------------*

$INCLUDE (RTXCONST.INC)
$INCLUDE (RTXEXTRN.INC)

;     FROM RTXDISP
EXTRN CODE (?RTX_REDISPATCH)


;*----------------------------------------------------------------------*
;*      E X P O R T S
;*----------------------------------------------------------------------*

PUBLIC  _os_create_pool
PUBLIC  _os_get_block
PUBLIC  _os_free_block


;*----------------------------------------------------------------------*
;*      P R O C E D U R E S
;*----------------------------------------------------------------------*

;*----------------------------------------------------------------------*
;*      U N S I G N E D _ I N T _ D I V
;*----------------------------------------------------------------------*
;*      Performs a division of two unsigned integers
;*----------------------------------------------------------------------*
;*
;*                 R6/R7
;*      R6/R7 = -----------
;*                 R4/R5
;*
;*     This Procedure was extracted from the Keil C-51 Runtime-Library.
;*
;*     Used registers:
;*         R0, R4, R5, R6, R7, A, B, PSW
;*----------------------------------------------------------------------*

?RTX?UNSIGNED_INT_DIV?RTXPOOL    SEGMENT CODE
                                 RSEG ?RTX?UNSIGNED_INT_DIV?RTXPOOL

UNSIGNED_INT_DIV:
                CJNE      R4,#00H,L1
                CJNE      R6,#00H,L2
                MOV       A,R7
                MOV       B,R5
                DIV       AB
                MOV       R7,A
                MOV       R5,B
                RET
L1:
                CLR       A
                XCH       A,R4
                MOV       R0,A
                MOV       B,#08H
L5:
                MOV       A,R7
                ADD       A,R7
                MOV       R7,A
                MOV       A,R6
                RLC       A
                MOV       R6,A
                MOV       A,R4
                RLC       A
                MOV       R4,A
                MOV       A,R6
                SUBB      A,R5
                MOV       A,R4
                SUBB      A,R0
                JC        L4
                MOV       R4,A
                MOV       A,R6
                SUBB      A,R5
                MOV       R6,A
                INC       R7
L4:
                DJNZ      B,L5
                CLR       A
                XCH       A,R6
                MOV       R5,A
                RET

L2:
                MOV       A,R5
                MOV       R0,A
                MOV       B,A
                MOV       A,R6
                DIV       AB
                JB        PSW.2,L6              ; test the overflow flag
                MOV       R6,A
                MOV       R5,B
                MOV       B,#08H
L9:
                MOV       A,R7
                ADD       A,R7
                MOV       R7,A
                MOV       A,R5
                RLC       A
                MOV       R5,A
                JC        L7
                SUBB      A,R0
                JNC       L8
                DJNZ      B,L9
                RET
L7:
                CLR       C
                SUBB      A,R0
L8:
                MOV       R5,A
                INC       R7
                DJNZ      B,L9
L6:
                RET



;*----------------------------------------------------------------------*
;*      O S _ C R E A T E _ P O O L
;*----------------------------------------------------------------------*
;*      Implements the os_create_pool system function
;*----------------------------------------------------------------------*
;*      Input:  - R6/R7: size for each block in pool (1..FFFFH)
;*      -----   - R4/R5: start-address for the pool
;*              - R2/R3: memory-size reserved for the pool
;*
;*      Output: - R7: Function result (OK, NOT_OK)
;*      ------
;*
;*----------------------------------------------------------------------*

?RTX?_os_create_pool?RTXPOOL  SEGMENT CODE
                              RSEG ?RTX?_os_create_pool?RTXPOOL

_os_create_pool:
               DBG_SYS_ENTRY
               RTX_EXCLUDE_INT
               SETB  ?RTX_SUPERVISOR_MODE       ; Supervisor_mode := on
               RTX_ALLOW_DRV_INT
               ;  Check the block-size
               MOV   A, R7
               ORL   A, R6
               JNZ   INPUT_OK1
               MOV   R7, #NOT_OK
               JMP   END_POOL

INPUT_OK1:     ; Search an unused pool-control structure
               ; The pool-control structures are inpage-allocated!
               RTX_EXCLUDE_INT
               MOV   R0, #0                     ; R0 -> contr.-struct. counter
               MOV   DPTR, #?RTX_POOL_CONTR
LOOP1:         MOVX  A, @DPTR                   ; check block_size
               MOV   B, A
               INC   DPL
               MOVX  A, @DPTR
               MOV   R1, A                      ; R1:Block-Size from pool-control
               MOV   A, R6
               CJNE  A, B, BS_OK
               MOV   A, B
               MOV   B, R1
               MOV   R1, A
               MOV   A, R7
               CJNE  A, B, BS_OK
               JMP   POOL_NOT_OK                ; Block-size already defined

BS_OK:         MOV   A, R1
               ORL   A, B
               JNZ   NEXT_CONT1                 ; block-size is <> 0
               INC   DPL                        ; check the pool start-addr.
               MOVX  A, @DPTR
               MOV   R1, A
               INC   DPL
               MOVX  A, @DPTR
               ORL   A, R1
               JNZ   NEXT_CONT1                 ; start-adr. is <> 0

               ; unused pool-control structure found, save the paramters
               DEC   DPL
               DEC   DPL
               DEC   DPL
               MOV   A, R6                      ; save the block-size
               MOVX  @DPTR, A
               INC   DPL
               MOV   A, R7
               MOVX  @DPTR, A
               INC   DPL
               MOV   A, R4                      ; save the start-adr.
               MOVX  @DPTR, A
               INC   DPL
               MOV   A, R5
               MOVX  @DPTR, A
               INC   DPL
               MOV   A, #NIL_BLOCK
               MOVX  @DPTR, A                   ; set the first free-block
                                                ; number (no free blocks
                                                ; until the pool is
                                                ; initialized)
               PUSH  DPH
               PUSH  DPL                        ; save the DPTR
               JMP   INIT_BLOCKS

NEXT_CONT1:    ; go to the next pool-control struct
               INC   R0
               MOV   A, R0
               SETB  C
               SUBB  A, #0FH
               JNC   POOL_NOT_OK                ; No unused pool-contr struct found
               MOV   A, R0
               MOV   B, #06H
               MUL   AB
               ADD   A, #LOW(?RTX_POOL_CONTR)
               MOV   DPL, A
               JMP   LOOP1

POOL_NOT_OK:   ; return with not ok
               MOV   R7, #NOT_OK
               JMP   END_POOL


INIT_BLOCKS:   RTX_ALLOW_DRV_INT
               RTX_EXCLUDE_INT                  ; Driver-window
               ; Save the needed parameters
               MOV   DPH, R4
               MOV   DPL, R5                    ; Init DPTR to pool-start
               MOV   A, R2
               MOV   R4, A
               MOV   A, R3
               MOV   R5, A                      ; R4/R5 holds the mem_size
               MOV   A, R6
               MOV   R1, A
               MOV   A, R7
               MOV   R2, A                      ; R1/R2 holds the block_size

               ; prepare the registers for UNSIGNED_INT_DIV
               MOV   A, R4
               MOV   R6, A
               MOV   A, R5
               MOV   R7, A                      ; mem_size now in R6/R7
               ; nbr_of_blocks = mem_size / (block_size + 2)
               MOV   A, R2
               ADD   A, #02H
               MOV   R5, A
               MOV   A, R1
               ADDC  A, #00H
               MOV   R4, A                      ; R4/R5 holds block_size+2
               ; make free 2 Bytes of stack for subroutine call
               POP   ?RTX_TMP1
               POP   ?RTX_TMP2
               CALL  UNSIGNED_INT_DIV
               PUSH  ?RTX_TMP2
               PUSH  ?RTX_TMP1
               ; R6/R7 holds the number of blocks
               ; R1/R2 holds the block_size
               ; DPTR holds the pool start_address

               RTX_ALLOW_DRV_INT
               ; limit the number of blocks to 255
               MOV   A, R7
               SETB  C
               SUBB  A, #0FFH
               MOV   A, R6
               SUBB  A, #00H
               JC    BLOCK_NBR_OK
               MOV   R6, #00H
               MOV   R7, #0FFH

BLOCK_NBR_OK:  MOV   A, R7
               JZ    NO_BLOCKS                  ; No space for blocks in pool
               MOV   R0, #1                     ; Init the block-counter

NEXT_BLOCK:    ; Check if all blocks are initialized
               MOV   A, R7
               CLR   C
               SUBB  A, R0
               JZ    LAST_BLOCK
               ; Init the block
               MOV   A, R0
               MOVX  @DPTR, A
               INC   DPTR
               MOV   A, #NIL_TID
               MOVX  @DPTR, A
               ; Calculate the new block-ptr
               INC   DPTR
               MOV   A, DPL
               ADD   A, R2
               MOV   DPL, A
               MOV   A, DPH
               ADDC  A, R1
               MOV   DPH, A
               INC   R0                         ; increment the block-counter
               JMP   NEXT_BLOCK

LAST_BLOCK:    MOV   A, #NIL_BLOCK
               MOVX  @DPTR, A
               INC   DPTR
               MOV   A, #NIL_TID
               MOVX  @DPTR, A
               ; give the blocks free
               RTX_EXCLUDE_INT
               POP   DPL
               POP   DPH
               CLR   A
               MOVX  @DPTR, A
               ; set the number of blocks
               INC   DPL
               MOV   A, R7
               MOVX  @DPTR, A
               RTX_ALLOW_DRV_INT
               ; return with ok
               MOV   R7, #OK
               JMP   END_POOL

NO_BLOCKS:      ; Release pool control structure (was already reserved)
               POP   DPL                        ; get back ctr struct ptr
               POP   DPH
               DEC   DPL
               CLR   A
               MOVX  @DPTR, A                   ; start-adr = 0
               DEC   DPL
               MOVX  @DPTR, A
               DEC   DPL
               MOVX  @DPTR, A                   ; block_size = 0
               DEC   DPL
               MOVX  @DPTR, A
               ; return with not ok
               MOV   R7, #NOT_OK
                
END_POOL:      RTX_EXCLUDE_INT
               JB    ?RTX_REQ_REDISPATCH, RED
               CLR   ?RTX_SUPERVISOR_MODE       ; supervisor_mode := OFF
               DBG_SYS_EXIT
               RTX_ALLOW_INT
               RET

RED:           CLR   ?RTX_FULL_CONTEXT
               JMP   ?RTX_REDISPATCH



;*----------------------------------------------------------------------*
;*      O S _ G E T _ B L O C K
;*----------------------------------------------------------------------*
;*      Implements the os_get_block system function
;*----------------------------------------------------------------------*
;*      Input:  - R6/R7: block_size of the pool (1..FFFFH)
;*      -----
;*
;*      Output: - R6/R7: =  0 -> no free block or the pool does not exists
;*      ------           <> 0 -> xdata-pointer to a block in the pool
;*
;*----------------------------------------------------------------------*

?RTX?_os_get_block?RTXPOOL  SEGMENT CODE
                            RSEG ?RTX?_os_get_block?RTXPOOL

_os_get_block:
               DBG_SYS_ENTRY
               RTX_EXCLUDE_INT
               SETB  ?RTX_SUPERVISOR_MODE       ; Supervisor_mode := on
               RTX_ALLOW_DRV_INT
               ;  Check the block-size
               MOV   A, R7
               ORL   A, R6
               JNZ   INPUT_OK2
               MOV   R6, #00H
               MOV   R7, #00H
               JMP   END_POOL

INPUT_OK2:     ; Search the coresponding pool-control structure
               ; The pool-control structures are inpage-allocated!
               MOV   R0, #0                     ; R0 -> contr.-struct. counter
               MOV   DPTR, #?RTX_POOL_CONTR
LOOP2:         MOVX  A, @DPTR                   ; check block_size
               XRL   A, R6
               JNZ   NEXT_CONT2                 ; block-size is not equal
               INC   DPL
               MOVX  A, @DPTR
               XRL   A, R7
               JNZ   NEXT_CONT2                 ; block-size is not equal

               ; pool-control structure found, load the pool_ptr
               ; and the next_free_block number
               RTX_EXCLUDE_INT
               INC   DPL
               MOVX  A, @DPTR
               MOV   R1, A
               INC   DPL
               MOVX  A, @DPTR
               MOV   R2, A                      ; R1/R2 holds the pool_ptr
               INC   DPL
               MOVX  A, @DPTR
               MOV   R5, A                      ; R5 holds the next_free_block
               ; Check the free block-number
               CJNE  A, #NIL_BLOCK, BLOCK_OK
               ; no more free blocks, return 0-pointer
               MOV   R6, #00H
               MOV   R7, #00H
               JMP   END_POOL

NEXT_CONT2:    ; go to the next pool-control struct
               INC   R0
               MOV   A, R0
               SETB  C
               SUBB  A, #0FH
               JNC   NOT_FOUND2                 ; No coresponding pool-contr struct found
               MOV   A, R0
               MOV   B, #06H
               MUL   AB
               ADD   A, #LOW(?RTX_POOL_CONTR)
               MOV   DPL, A
               JMP   LOOP2

NOT_FOUND2:    ; return with 0-pointer
               MOV   R6, #00H
               MOV   R7, #00H
               JMP   END_POOL

BLOCK_OK:      ; DPTR points to the next_free_block field in pool_control
               ; R1/R2 holds the pool_ptr (pool start address)
               ; R5 holds the next_free_block number
               ; calculate the blockaddress
               MOV   A, R7
               ADD   A, #02H
               MOV   R7, A
               MOV   A, R6
               ADDC  A, #00H
               MOV   R6, A                      ; block_size = block_size+2
               MOV   R4, #00H
               ; R6/R7 = next_free_block*block_size
               MOV   A, R7
               MOV   R0, A
               MOV   B, R5
               MUL   AB
               MOV   R7, A
               MOV   A, R5
               XCH   A, B
               XCH   A, R6
               MUL   AB
               ADD   A, R6
               MOV   R6, A
               MOV   A, R4
               MOV   B, R0
               MUL   AB
               ADD   A, R6
               MOV   R6, A
               ; Add this result to the pool-start-address
               MOV   A, R7
               ADD   A, R2
               MOV   R7, A
               MOV   A, R6
               ADDC  A, R1
               MOV   R6, A                      ; R6/R7 holds the block-addr
               MOV   R0, DPH
               MOV   R1, DPL                    ; save the DPTR to R0/R1
               MOV   DPH, R6
               MOV   DPL, R7
               MOVX  A, @DPTR                   ; load the next-block value
               MOV   R2, A                      ; to R2
               INC   DPTR
               MOV   A, ?RTX_RUNNING_TID
               MOVX  @DPTR, A                   ; save the owner of the block
               INC   DPTR
               MOV   R6, DPH
               MOV   R7, DPL                    ; set the return-value to the
                                                ; block-start address
               MOV   DPH, R0
               MOV   DPL, R1
               MOV   A, R2
               MOVX  @DPTR, A                   ; Update the next_free_block
                                                ; value in the pool-control
                                                ; struct

               RTX_ALLOW_DRV_INT                ; Driver window
               RTX_EXCLUDE_INT
               JB    ?RTX_REQ_REDISPATCH, RED1
               CLR   ?RTX_SUPERVISOR_MODE       ; supervisor_mode := OFF
               DBG_SYS_EXIT
               RTX_ALLOW_INT
               RET

RED1:          JMP   ?RTX_REDISPATCH



;*----------------------------------------------------------------------*
;*      O S _ F R E E _ B L O C K
;*----------------------------------------------------------------------*
;*      Implements the os_free_block system function
;*----------------------------------------------------------------------*
;*      Input:  - R6/R7: block_size of the pool (1..FFFFH)
;*      -----   - R4/R5: address of the block
;*
;*      Output: - R7: Function result (OK, NOT_OK)
;*      ------
;*
;*----------------------------------------------------------------------*

?RTX?_os_free_block?RTXPOOL  SEGMENT CODE
                             RSEG ?RTX?_os_free_block?RTXPOOL

_os_free_block:
               DBG_SYS_ENTRY
               RTX_EXCLUDE_INT
               SETB  ?RTX_SUPERVISOR_MODE       ; Supervisor_mode := on
               RTX_ALLOW_DRV_INT
               ;  Check the block-size
               MOV   A, R7
               ORL   A, R6
               JNZ   INPUT_OK3
               MOV   R7, #NOT_OK
               JMP   END_POOL

INPUT_OK3:     ; Search the coresponding pool-control structure
               ; The pool-control structures are inpage-allocated!
               MOV   R0, #0                     ; R0 -> contr.-struct.counter
               MOV   DPTR, #?RTX_POOL_CONTR
LOOP3:         MOVX  A, @DPTR                   ; check block_size
               XRL   A, R6
               JNZ   NEXT_CONT3                 ; block-size is not equal
               INC   DPL
               MOVX  A, @DPTR
               XRL   A, R7
               JNZ   NEXT_CONT3                 ; block-size is not equal

               ; pool-control structure found, load the the next_free_block
               ; number
               RTX_EXCLUDE_INT
               INC   DPL
               MOV   R1, DPH
               MOV   R2, DPL                    ; save the DPTR (pointer
                                                ; to the pool_ptr
                                                ; field)
               INC   DPL
               INC   DPL
               MOVX  A, @DPTR
               MOV   R0, A                      ; R0 holds the next_free_block

               ; Decrement the block-address by 1
               MOV   A, R5
               ADD   A, #0FFH
               MOV   R5, A
               MOV   A, R4
               ADDC  A, #0FFH
               MOV   R4, A
               MOV   DPH, R4
               MOV   DPL, R5
               MOVX  A, @DPTR                   ; load the owner field
               CJNE  A, #NIL_TID, BLOCK_OCCUPIED
               ; Block is not used, error
               MOV   R7, #NOT_OK
               JMP   END_POOL

NEXT_CONT3:    ; go to the next pool-control struct
               INC   R0
               MOV   A, R0
               SETB  C
               SUBB  A, #0FH
               JNC   NOT_FOUND3                 ; No coresponding pool-contr struct found
               MOV   A, R0
               MOV   B, #06H
               MUL   AB
               ADD   A, #LOW(?RTX_POOL_CONTR)
               MOV   DPL, A
               JMP   LOOP3

NOT_FOUND3:    ; return with NOT_OK
               MOV   R7, #NOT_OK
               JMP   END_POOL


BLOCK_OCCUPIED:
               ; set the owner field
               MOV   A, #NIL_TID
               MOVX  @DPTR, A
               ; Decrement the block-address by 1
               MOV   A, R5
               ADD   A, #0FFH
               MOV   R5, A
               MOV   A, R4
               ADDC  A, #0FFH
               MOV   R4, A
               MOV   DPH, R4
               MOV   DPL, R5
               ; set the next-block field
               MOV   A, R0
               MOVX  @DPTR, A
               ; Calculate the actual block-number
               ; (R1/R2 points to the pool_ptr)
               MOV   DPH, R1
               MOV   DPL, R2
               MOVX  A, @DPTR
               MOV   R0, A
               INC   DPL
               MOVX  A, @DPTR
               MOV   R1, A                      ; R0/R1 holds the pool_ptr
               ; block_address = block_address-pool_ptr
               CLR   C
               MOV   A, R5
               SUBB  A, R1
               MOV   R5, A
               MOV   A, R4
               SUBB  A, R0
               MOV   R4, A
               ; prepare for UNSIGNED_INT_DIV routine
               MOV   A, R4
               XCH   A, R6
               MOV   R4, A
               MOV   A, R5
               XCH   A, R7
               MOV   R5, A
               MOV   A, R5
               ADD   A, #02H
               MOV   R5, A
               MOV   A, R4
               ADDC  A, #00H
               MOV   R4, A
               ; make free 2 Bytes of stack for subroutine call
               POP   ?RTX_TMP1
               POP   ?RTX_TMP2
               CALL  UNSIGNED_INT_DIV
               PUSH  ?RTX_TMP2
               PUSH  ?RTX_TMP1
               ; Save the result to the next_free_block field
               INC   DPTR
               MOV   A, R7
               MOVX  @DPTR, A
               ; return with ok
               MOV   R7, #OK
               JMP   END_POOL


;
;   END OF MODULE
;
        END

